One prior art terrain aided navigation system is available from Sandia Labs. Sandia has created the Sandia Inertial Terrain-Aided Navigation (SITAN) flight-computer algorithm that produces a very accurate trajectory for low-flying, high-performance aircraft by combining outputs from a radar or laser altimeter, an inertial navigation system (INS), and a digital terrain elevation map. SITAN is a recursive, real time, terrain-aided navigation algorithm for use on fighter aircraft. The algorithm has been implemented in a popular microprocessor. The aircraft's position can be estimated within a 926 meter circle error of probability. A good description of the SITAN terrain aided navigation system can be found in the proceedings of the IEEE National Aerospace and Electronics Conference--NAECON, May 20-24, 1985 entitled The AFTI/F16 Terrain-Aided Navigation System, by D. D. Boozer, M. K. Lau, J. R. Fellerhoff, Sandia National Laboratories, Albuquerque, N. Mex. 87185.
SITAN utilizes Kahan filter algorithms to process geophysical measurements. The algorithm estimates errors in a flight path produced by an inertial navigation system following the equations given in the above-mentioned article.
The Kahan filter can be formed with the following state model: EQU 67 X.sub.k+1 =.phi..delta.X.sub.k +W
and the measurement ##EQU1## where .delta.X.sub.k =INS error states to be estimated
.PHI.=state-transition matrix for INS errors PA1 X.sub.k =states of INS and aircraft PA1 C.sub.k =ground clearance measurement PA1 Z.sub.k =altitude of aircraft PA1 h =height of terrain at position ( .,. ) PA1 W.sub.k =driving noise with E(W.sub.k)=0 for all k and E(W.sub.k W.sub.j.sup.T)=Q.sub.k .delta..sub.kj PA1 V.sub.k =measurement error with E(V.sub.k)=0 for all k and E(V.sub.k V.sub.j)=R.sub.k .delta..sub.kj PA1 k=subscript denoting time k.
Since the measurement function c(x) is a nonlinear function of the states, the standard extended Kahan filter approach is used to obtain the measurement matrix, ##EQU2## where h.sub.x and h.sub.y are the terrain slopes in the x and y directions of the map evaluated at X.sub.k (-), the predicted aircraft position just before a measurement is processed at time k. The first three states are taken to be the x position, y position, and altitude, respectively. At any time k, EQU X=X.sub.INS +.delta.X
The state vector often used in a single filter implementation is EQU .delta.X=[.delta.X .delta.Y .delta.Z .delta.V.sub.X .delta.V.sub.Y ].sup.T
where .delta.X, .delta.Y, .delta.Z, .delta.V.sub.x, and .delta.V.sub.Y are errors in the X position, Y position, altitude, X velocity, and Y velocity, respectively. Other INS errors and states can also be included in .delta.X by using the proper .PHI..
Parallel SITAN was developed because the distance needed by SITAN to reach steady state becomes excessive as initial position errors (IPEs) approach several hundred meters. Parallel SITAN is a bank of extended Kahan filters that process identical altimeter measurements. After some updates, the filter with the minimum average weighted residual squared (AWRS) value is identified as having the correct position estimate. The AWRS value is defined by ##EQU3## where .DELTA..sub.i is the residual at the ith update, n is the number of updates, and HPH.sup.T +R is the residual variance. Once the IPEs are reduced by the parallel filters, a single filter performs well, starting off essentially in steady state.
To implement parallel SITAN, the number and geometrical layout of the parallel filters needed to cover an IPE must be specified. A square, constant-spaced grid can be used to center the filters about the horizontal position indicated by the INS. Filters at and near the corners are then eliminated to reduce the number of filters. To further lighten the computational burden, three-state, instead of five-state, filters are often used in parallel SITAN with EQU .delta.X=[.delta.X .delta.Y .delta.Z].sup.T
For both the single and parallel filter implementation, a least-squares plane fit to the map, known as stochastic linearization, is used to compute the slopes, h.sub.x and H.sub.Y. Horizontal uncertainties .sigma..sub.X and .sigma..sub.Y from the error-covariance matrix, defined by ##EQU4## are used to determine the size of the plane. Residuals from the plane fit, RFIT.sub.k, are added to the measurement error variance, R.sub.k, to ensure that the SITAN filter assigns less weight to the measurement when the plane fit is either very large or is over a rough area, thus adapting to local terrain.
SITAN has three basic modes: acquisition mode, lost mode and track mode.
Acquisition Mode
When one of the filters identifies a reliable estimate of true aircraft position, the track mode is entered. A single five state Kahan filter is initialized at the estimated acquired position. During track mode, the aircraft's position is estimated every 100 meters. SITAN employs a mode control logic concept to handle the transition from track to lost and from acquisition to track.
Acquisition mode is used to locate the aircraft's position within a circular region of uncertainty. The region of uncertainty is called the circular error of probability or CEP. In SITAN a 2100 meter initial position CEP is covered with 57 3-state Kalman filters centered on a grid whose initial positions are 525 meters apart.
The state acquisition filters include error states. They can estimate an aircraft's position under significant INS velocity errors. Each filter is initialized with position error estimates. To ensure that erroneous information is not employed to update the filters, none of the filters are updated if the pitch altitude is greater than 30 degrees or if the altimeter loses lock or the INS fails.
If the mode control logic of SITAN indicates that the aircraft has deviated from the 2363 meter search area, the lost mode is initiated. The lost mode ceases to provide position estimates for the aircraft, and the pilot has to update the inertial navigation before restarting SITAN.
Track Mode
Track mode estimates the position of the aircraft during flight. The five state tracking filter is used. The acquisition mode initializes the track filters. The track mode makes estimates of terrain slopes in rectangles surrounding the center of the aircraft position. The track filters are not updated during track mode unless the inertial navigation system, altimeter, and other flight data are valid.
Track mode is entered when the aircraft's actual position is close to the estimated one. In the event of a false fix, the track filter is set at the wrong position and the SITAN algorithm will proceed as if it was correct. This produces large errors in estimated positions. It is imperative that the probability of a false fix be kept low. The following is taken from the above-mentioned article in regard to the mode control logic of SITAN.
With the design for the acquisition, lost, and track modes as described above, the mode-control logic is needed to determine in which mode the algorithm should operate. When large aircraft position errors exist, it should choose the acquisition mode; with small errors, the track. The main parameter used in the mode-control logic for transition from acquisition to track is the AWRS. FIG. 7 shows a cross-section of a three-dimensional AWRS surface where AWRS is a function of the parallel filter positions (X.sub.j, Y.sub.j). In the acquisition mode the parallel filters will tend to migrate to the relative minima of this surface. PA0 To keep the parallel filters from migrating too far from their initial positions, a maximum of 128 updates is allowed. Four tests are performed after every 32 updates to determine if transition to the track mode is possible by selecting the acquisition filter with the minimum AWRS. Test 1 requires the selected filter to have an AWRS lower than a threshold value to ensure that the parallel filters are indeed over the correct aircraft position. If the parallel filters are configured over an area which does not include the true aircraft position, the global minimum of the AWRS curve is expected to shift upward. Test 2 requires contrast in the terrain, a sufficient difference between AWRS.sub.min and AWRS.sub.max to prevent transition to the track mode over very smooth areas such as water. Test 3, the false-fix test, requires that the minimum AWRS outside of an exclusion region, AWRS*.sub.min does not compete with AWRS.sub.min, where the size of the exclusive region is computed using .sigma..sub.X and .sigma..sub.Y of the selected filter. Statistically, as more updates are made, the separation between the global minimum and relative minima can be realized and still retain the same confidence level. Therefore, the required separation between AWRS*.sub.min and AWRS.sub.min should be a function of 1/n, where n is the number of updates. Test 4 requires the .sigma..sub.X and .sigma..sub.Y of the selected filter to be smaller than its initial value, 200 m, indicating that improvements have been made in estimating the aircraft's position during acquisition. PA0 If track is not entered after 128 updates, a test for the lost mode is made. If AWRS.sub.min is greater than a threshold, the lost mode is entered and AFTI/SITAN will not produce any more estimates until the pilot updates the INS, which will automatically restart AFTI/SITAN in the track mode. If the lost condition is not met, the parallel filters are reset and acquisition starts over. This process continues until track or lost mode is entered. PA0 Once in track, the track filter's .sigma..sub.X and .sigma..sub.Y are checked continuously while its AWRS is checked every 64 updates. If either the .sigma.'s or the AWRS is too high, AFTI/SITAN switches back to the acquisition mode. The test on .sigma. prevents the track filter from using excessively large plane fits to maintain track after flying over smooth terrain for a long time. The AWRS test checks for unbelievable filter residuals. PA0 Another approach is to introduce a wood model in terms of a state s and an equation, say ##EQU5## the output relation is then EQU .zeta.=z-h(x,y)-s+e.sub.m PA0 e.sub.ss has an assumed variance Q.sub.s. As Q.sub.s increases, the system will use .zeta. more and more suspiciously, which makes the system less and less sensitive to such disturbances as woods. Finally when Q.sub.s is very large the system will not dare to draw any conclusions at all from .zeta.. The position will then diverge at a rate determined by the inertial navigation system.